#include "pid.h"
#include <math.h>
#include <stdlib.h>
/**
 * @brief  PID控制器初始化
 * @param  pid: PID控制器结构体指针
 * @param  kp: 比例系数
 * @param  ki: 积分系数
 * @param  kd: 微分系数
 * @param  output_min: 输出最小值
 * @param  output_max: 输出最大值
 * @retval 无
 */
void PID_Init(PID_Controller *pid, float kp, float ki, float kd, float output_min, float output_max) {
    if (pid == NULL) {
        return;
    }
    
    // 初始化PID参数
    pid->kp = kp;
    pid->ki = ki * 0.1f;  // 考虑采样时间
    pid->kd = kd / 0.1f;  // 考虑采样时间

    // 初始化限制值
    pid->output_min = output_min;
    pid->output_max = output_max;
    pid->integral_limit = fabsf(output_max);
    
    // 初始化状态变量
    pid->setpoint = 0.0f;
    pid->feedback = 0.0f;
    pid->error = 0.0f;
    pid->last_error = 0.0f;
    pid->integral = 0.0f;
    pid->derivative = 0.0f;
    pid->output = 0.0f;
}

/**
 * @brief  PID计算函数
 * @param  pid: PID控制器结构体指针
 * @param  feedback: 当前反馈值
 * @retval PID输出值
 */
float PID_Compute(PID_Controller *pid, float feedback) {

    // 更新反馈值
    pid->feedback = feedback;
    // 计算误差
    pid->error = pid->setpoint - pid->feedback;

    // 计算积分项（带积分限幅和抗积分饱和）
        pid->integral += pid->ki * pid->error;
        // 积分限幅
        if (pid->integral > pid->integral_limit) {
            pid->integral = pid->integral_limit;
        } else if (pid->integral < -pid->integral_limit) {
            pid->integral = -pid->integral_limit;
        }
    // if (fabs(pid->error) > 100.0f)pid->integral = 0.0f;

    // 计算微分项（微分先行）
    pid->derivative = pid->kd * pid->error ;
    // 保存当前误差用于下次计算
    pid->last_error = pid->error;
    // 计算PID输出
    // if  (fabs(pid->error) > 100.0f)pid->output = (pid->kp*2.0f * pid->error) + pid->integral + pid->derivative;
   pid->output = (pid->kp * pid->error) + pid->integral + pid->derivative;

    pid->output += pid->setpoint*5.0f;
    // 输出限幅
    if (pid->output > pid->output_max) {
        pid->output = pid->output_max;
    } else if (pid->output < pid->output_min) {
        pid->output = pid->output_min;
    }
    return pid->output;
}

/**
 * @brief  设置目标值
 * @param  pid: PID控制器结构体指针
 * @param  setpoint: 目标值
 * @retval 无
 */
void PID_SetSetpoint(PID_Controller *pid, float setpoint) {
    if (pid == NULL) {
        return;
    }
    pid->setpoint = setpoint;
}

/**
 * @brief  重置PID控制器
 * @param  pid: PID控制器结构体指针
 * @retval 无
 */
void PID_Reset(PID_Controller *pid) {
    if (pid == NULL) {
        return;
    }
    
    pid->last_error = 0.0f;
    pid->integral = 0.0f;
    pid->derivative = 0.0f;
    pid->output = 0.0f;
}
